﻿#include "drawpathwidget.h"
#include <osgGA/TrackballManipulator>       /// osgGA::TrackballManipulator
#include <osgViewer/ViewerEventHandlers>    /// osgViewer::StatsHandler
#include <osg/LineWidth>
#include <osg/Node>
#include <osg/Geode>
#include <osg/Group>
#include <osgDB/WriteFile>
#include <osgUtil/Optimizer>
#include <QtWidgets>
#include <ctime>                            /// qsrand(time(NULL));   qrand()
#include <limits.h>                         /// FLT_MAX FLT_MIN
#include "global.h"                         /// FindNodeWithName
#include <osgUtil/IntersectionVisitor>      /// osgUtil::IntersectionVisitor相交器访问器
#include <Eigen/Dense>                      /// Eigen::Quaterniond


#include "eventinteraction.h"


const int zAxisScale = 10;   /// 扩大z轴长度系数
const int splitNum = 10;     /// 将坐标轴分割为splitNum段


/// 这三变量有重名函数 所以加了命名空间
namespace global {

    float width;
    float depth;
    float height;

}


/// 一些全局变量
float xBegin;
float xEnd;
float yBegin;
float yEnd;
float zBegin;
float zEnd;



DrawPathWidget::DrawPathWidget(QWidget *parent) : QWidget(parent),
    pViewer(0),
    pWindow(0),
    pointsArray(new osg::Vec3Array),
    fileName("")
{
    init();
    layout();
}


DrawPathWidget::~DrawPathWidget()
{

}


/** 设置轨迹数据并画出轨迹
 * @points 解析得到的轨迹点数组
*/
void DrawPathWidget::setPathPoint(const std::vector<std::vector<float>>& points)
{
#ifndef NDEBUG

    qDebug() << "DrawPathWidget::setPathPoint";

#endif

    if (points.empty()) return;



    static bool isFirstInit = true;

    /// 第一遍遍历确定轨迹边界,只在第一次导入数据的时候执行,只执行一次,因为所以轨迹都映射到第一条轨迹所建立的空间范围内
    if (isFirstInit) {

        XMinLimit = FLT_MAX, XMaxLimit = -FLT_MAX;
        YMinLimit = FLT_MAX, YMaxLimit = -FLT_MAX;
        ZMinLimit = FLT_MAX, ZMaxLimit = -FLT_MAX;

        isFirstInit = false;
        for (auto point: points) {

            XMinLimit = std::min(XMinLimit, point[0]);
            XMaxLimit = std::max(XMaxLimit, point[0]);
            YMinLimit = std::min(YMinLimit, point[1]);
            YMaxLimit = std::max(YMaxLimit, point[1]);
            ZMinLimit = std::min(ZMinLimit, point[2]);
            ZMaxLimit = std::max(ZMaxLimit, point[2]);

        }

        /// 根据第一条轨迹初始化一个能容纳它的坐标系加入到根节点中
        osg::ref_ptr<osg::Group> root = pViewer->getSceneData()->asGroup();
        osg::ref_ptr<osg::Geode> coordinate = initCoordinate(XMinLimit, XMaxLimit, YMinLimit, YMaxLimit, ZMinLimit, ZMaxLimit, splitNum);
        root->addChild(coordinate);

        /// 轨迹在z坐标轴中间的占比 例如: 1为100% 3为33% 5为20%
        const int n = 1;

        /// 设置映射空间
        setMapping(XMinLimit, XMaxLimit, YMinLimit, YMaxLimit, ZMinLimit, ZMaxLimit, n);

    }

    /// 第二遍遍历更新容器, 根据点的范围通过映射将每个点映射到 width*depth*height 的空间范围内的中间n分之一处, n为奇数
    this->pointsArray = new osg::Vec3Array(0);


    for (auto point: points) {

        this->pointsArray->push_back(mapping(point));

    }

    drawPath();
    this->fileName = "";

}



void DrawPathWidget::setMapping(float XMinLimit, float XMaxLimit, float YMinLimit, float YMaxLimit,
                                float ZMinLimit, float ZMaxLimit, int n = 1)
{

    this->XMinLimit = XMinLimit;
    this->XMaxLimit = XMaxLimit;
    this->YMinLimit = YMinLimit;
    this->YMaxLimit = YMaxLimit;
    this->ZMinLimit = ZMinLimit;
    this->ZMaxLimit = ZMaxLimit;
    this->n = n;


     global::width = XMaxLimit - XMinLimit;
     global::depth = YMaxLimit - YMinLimit;
     global::height = ZMaxLimit - ZMinLimit;
     xBegin = global::width*(n/2) / n;        /// 映射后轨迹点在x轴最小值
     xEnd = global::width*(n/2+1) / n;        /// 映射后轨迹点在x轴最大值
     yBegin = global::depth*(n/2) / n;
     yEnd = global::depth*(n/2+1) / n;
     zBegin = global::height*(n/2) / n;
     zEnd = global::height*(n/2+1) / n;

}



/** 获取映射后的轨迹点坐标, 同时转换轨迹点数据类型
 * @point 实际轨迹点
 * @return 映射轨迹点
*/
osg::Vec3f DrawPathWidget::mapping(std::vector<float> point)
{

    osg::Vec3f res;
    res.x() = (point[0]-XMinLimit)/global::width*(xEnd-xBegin) + xBegin;     /// 映射后的x轴坐标
    res.y() = (point[1]-YMinLimit)/global::depth*(yEnd-yBegin) + yBegin;     /// 映射后的y轴坐标
    res.z() = (point[2]-ZMinLimit)/global::height*(zEnd-zBegin) + zBegin;    /// 映射后的z轴坐标

    /// 轨迹z轴放大系数 提高轨迹波动幅度
    res.z() *= zAxisScale;

    return res;

}



/** 获取映射前的轨迹点坐标
 * @point 映射轨迹点
 * @return 实际轨迹点
*/
osg::Vec3f DrawPathWidget::unMapping(osg::Vec3f point)
{

    osg::Vec3f res;
    res.x() = (point[0] - xBegin) * global::width/(xEnd-xBegin) + XMinLimit;
    res.y() = (point[1] - yBegin) * global::depth/(yEnd-yBegin) + YMinLimit;
    res.z() = (point[2] - zBegin) * global::height/(zEnd-zBegin) + ZMinLimit;

    res.z() /= zAxisScale;

    return res;

}


void DrawPathWidget::init()
{
#ifndef NDEBUG
    qDebug() << "DrawPathWidget::init()";
#endif
    this->pWindow = createGraphicsWindow(0, 0, 640, 480);
    this->pViewer = osg::ref_ptr<osgViewer::Viewer>(new osgViewer::Viewer);
    const osg::GraphicsContext::Traits* traits = pWindow->getTraits();
    osg::Camera* camera = pViewer->getCamera();
    camera->setGraphicsContext(pWindow);
    camera->setClearColor(osg::Vec4(0.2, 0.2, 0.6, 1.0) );
    camera->setViewport(new osg::Viewport(0, 0, traits->width, traits->height));
    camera->setProjectionMatrixAsPerspective(30.0f,
                                             static_cast<double>(traits->width)/static_cast<double>(traits->height),
                                             1.0f, 10000.0f );
#if 0
    /// 测试用
    osg::ref_ptr<osg::Node> node = osgDB::readNodeFile("cow.osg");
    pViewer->setSceneData(node);
#else
    /// root节点最好用osg::Group,因为只有组件点才能添加子节点
    /// 而几何节点osg::Geode添加子节点编译能通过但运行程序崩溃 这bug找了好久!! 凑!!
    osg::ref_ptr<osg::Group> root = new osg::Group;
    pViewer->setSceneData(root);
#endif

    osg::ref_ptr<osg::Viewport> viewport = new osg::Viewport(0, 0, this->width(), this->height());
    camera->setViewport(viewport);

#if 1
    /// 使用标准的事件类
    pViewer->addEventHandler(new osgViewer::StatsHandler);
#else
    /// 使用自定义的事件类 能够实现鼠标获取节点坐标
    pViewer->addEventHandler(new EventInteraction);
#endif

    /// 定义漫游器
    pViewer->setCameraManipulator(new osgGA::TrackballManipulator);

    /// 视口设为单线程（Qt5必须）
    pViewer->setThreadingModel(osgViewer::Viewer::SingleThreaded);

    QVBoxLayout* layout = new QVBoxLayout;
    layout->addWidget(pWindow->getGLWidget());
    setLayout(layout);

    /// 定时器结束时自动调用刷新界面函数
    connect(&timer, SIGNAL(timeout()), this, SLOT(update()));
    timer.start(10);
}



void DrawPathWidget::layout()
{

}



/* 初始化坐标轴
 * @xMinLimit xMaxLimit 轨迹点中x方向最小值与最大值, 即x坐标刻度范围
 * @splitNum 将坐标轴分割为n段
*/
osg::ref_ptr<osg::Geode> DrawPathWidget::initCoordinate(const float& xMinLimit = 0, const float& xMaxLimit = 100,
                                                        const float& yMinLimit = 0, const float& yMaxLimit = 100,
                                                        const float& zMinLimit = 0, const float& zMaxLimit = 100,
                                                        const int& splitNum = 10)
{

    // 步骤一: 创建用户自定义几何节点
    osg::ref_ptr<osg::Geode> geode = new osg::Geode;

    // 步骤二: 定义几何信息
    osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;

    // 步骤三: 设置坐标点数组
    osg::ref_ptr<osg::Vec3Array> vex = new osg::Vec3Array;
    const float width = xMaxLimit - xMinLimit;
    const float depth = yMaxLimit - yMinLimit;
    const float height = zMaxLimit - zMinLimit;
    const float xStep = width / splitNum;
    const float yStep = depth / splitNum;
    const float zStep = height / splitNum;

    for (float i = 0; i <= width; i += xStep) {
        vex->push_back(osg::Vec3(i, 0, 0));
        vex->push_back(osg::Vec3(i, depth, 0));
        vex->push_back(osg::Vec3(i, 0, 0));
        vex->push_back(osg::Vec3(i, 0, height*zAxisScale));

        // 显示刻度
        osg::ref_ptr<osg::Geode> pGeode = new osg::Geode;
        osg::ref_ptr<osgText::Text> pText = new osgText::Text;
        pText->setColor(osg::Vec4f(1.0, 1.0, 1.0, 1.0));        // 设置字体颜色
        pText->setPosition(osg::Vec3f(i, 0, 0));                // 设置字体位置
        pText->setCharacterSize(depth/20);                      // 设置字体大小
        pText->setAxisAlignment(osgText::Text::XY_PLANE);       // 设置字体附着于XY轴平面
        pText->setAlignment(osgText::Text::RIGHT_CENTER);       // 设置字体基点为右侧中间
        pText->setLayout(osgText::Text::LEFT_TO_RIGHT);         // 设置字体从左到右显示
        pText->setText(QString::number(i, 'f', 2).toStdString(), osgText::String::ENCODING_UTF8);   // 格式化数字保留小数小后两位
        geode->addDrawable(pText);
    }

    for (float i = 0; i <= depth; i += yStep) {
        vex->push_back(osg::Vec3(0, i, 0));
        vex->push_back(osg::Vec3(0, i, height*zAxisScale));
        vex->push_back(osg::Vec3(0, i, 0));
        vex->push_back(osg::Vec3(width, i, 0));
        // 显示刻度
        osg::ref_ptr<osg::Geode> pGeode = new osg::Geode;
        osg::ref_ptr<osgText::Text> pText = new osgText::Text;
        pText->setColor(osg::Vec4f(1.0, 1.0, 1.0, 1.0));
        pText->setPosition(osg::Vec3f(0, i, 0));
        pText->setAxisAlignment(osgText::Text::XY_PLANE);
        pText->setCharacterSize(depth/20);
        pText->setAlignment(osgText::Text::LEFT_CENTER);
        pText->setLayout(osgText::Text::LEFT_TO_RIGHT);
        pText->setText(QString::number(i, 'f', 2).toStdString(), osgText::String::ENCODING_UTF8);
        geode->addDrawable(pText);
    }

    for (float i = 0; i <= height; i += zStep) {
        vex->push_back(osg::Vec3(0, 0, i*zAxisScale));
        vex->push_back(osg::Vec3(0, depth, i*zAxisScale));
        vex->push_back(osg::Vec3(0, 0, i*zAxisScale));
        vex->push_back(osg::Vec3(width, 0, i*zAxisScale));
        // 显示刻度
        osg::ref_ptr<osg::Geode> pGeode = new osg::Geode;
        osg::ref_ptr<osgText::Text> pText = new osgText::Text;
        pText->setColor(osg::Vec4f(1.0, 1.0, 1.0, 1.0));
        pText->setPosition(osg::Vec3f(0, 0, i*zAxisScale));
        pText->setCharacterSize(depth/20);
        pText->setAxisAlignment(osgText::Text::XZ_PLANE);
        pText->setAlignment(osgText::Text::LEFT_CENTER);
        pText->setLayout(osgText::Text::LEFT_TO_RIGHT);
        pText->setText(QString::number(i, 'f', 2).toStdString(), osgText::String::ENCODING_UTF8);
        geode->addDrawable(pText);
    }
    geometry->setVertexArray(vex);

    // 步骤四: 设置坐标轴颜色数组
    osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array;
    colors->push_back(osg::Vec4(1.0, 1.0, 1.0, 1));
    geometry->setColorArray(colors);
    geometry->setColorBinding(osg::Geometry::BIND_OVERALL);

    // 步骤五: 定义绘图方式
    osg::ref_ptr<osg::PrimitiveSet> primitiveSet = new osg::DrawArrays(osg::PrimitiveSet::LINES , 0, vex.get()->size());
    geometry->addPrimitiveSet(primitiveSet);

    // 步骤六: 设置线宽
    osg::ref_ptr<osg::LineWidth> lineWidth = new osg::LineWidth(1.0);
    geometry->getOrCreateStateSet()->setAttribute(lineWidth, osg::StateAttribute::ON);

    // 步骤七: 添加几何信息到几何节点中
    geode->addDrawable(geometry);

    return geode;

}


osg::ref_ptr<osgQt::GraphicsWindowQt> DrawPathWidget::createGraphicsWindow(int x, int y, int w, int h)
{

    osg::ref_ptr<osg::GraphicsContext::Traits> traits = new osg::GraphicsContext::Traits;
    traits->windowDecoration = false;
    traits->x = x;
    traits->y = y;
    traits->width = w;
    traits->height = h;
    traits->doubleBuffer = true;
    return osg::ref_ptr<osgQt::GraphicsWindowQt>(new osgQt::GraphicsWindowQt(traits.get()));
}



/** 根据自身缓存的轨迹数组绘制轨迹 */
void DrawPathWidget::drawPath()
{

#ifndef NDEBUG

        qDebug() << QStringLiteral("DrawPathWidget::drawPath");

#endif

    if (this->pointsArray->size() == 0) return;

    /// 步骤一: 创建用户自定义几何节点
    osg::ref_ptr<osg::Geode> geode = new osg::Geode;


#if 0   /// 将轨迹绘制成线

    /// 步骤二: 定义几何信息
    osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;

    /// 步骤三: 设置绘制轨迹点数组
    geometry->setVertexArray(pointsArray);

    /// 步骤四: 设置颜色数组
    osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array;
    qsrand(time(NULL));
    int n = qrand() % 5+1;
    float random = 1.0 / n;
    colors->push_back(osg::Vec4(random, 1-random, random, 1));
    geometry->setColorArray(colors);
    geometry->setColorBinding(osg::Geometry::BIND_OVERALL);

    /// 步骤五: 定义绘图方式
    osg::ref_ptr<osg::PrimitiveSet> primitiveSet = new osg::DrawArrays(osg::PrimitiveSet::LINE_STRIP , 0, pointsArray->size());
    geometry->addPrimitiveSet(primitiveSet);

    /// 步骤六: 设置线宽
    osg::ref_ptr<osg::LineWidth> lineWidth = new osg::LineWidth(5.0);
    geometry->getOrCreateStateSet()->setAttribute(lineWidth, osg::StateAttribute::ON);

    /// 步骤七: 添加几何信息到几何节点中
    geode->addDrawable(geometry);

#else   /// 将轨迹绘制成细小圆柱体

    for (int i = 0; i < pointsArray->size()-1; ++i) {

        /// 根据起点和终点绘制圆柱体渲染对象
        osg::ref_ptr<osg::ShapeDrawable> capsuleDrawable = getCapsuleDrawableWithPoint(pointsArray->at(i), pointsArray->at(i+1));

        /// 将图形渲染对象加入到几何节点中
        geode->addDrawable(capsuleDrawable);

    }

#endif

    /// 步骤八: 节点加入根节点(坐标系)中
    geode->setName(fileName);
    pViewer->getSceneData()->asGroup()->addChild(geode);

}



osg::ref_ptr<osg::ShapeDrawable> DrawPathWidget::getCapsuleDrawableWithPoint(const osg::Vec3d& point1, const osg::Vec3d& point2)
{
    /// 创建几何体的精细度
    static osg::ref_ptr<osg::TessellationHints> pHints = new osg::TessellationHints;
    pHints->setDetailRatio(0.5);

    /// 设置胶囊体的半径
    double radius = 0.1f;

    /// 起点指向终点的向量
    osg::Vec3d vec1(0.0, 0.0, 1.0);
    osg::Vec3d vec2 = point2 - point1;
    double len = vec2.length();

    /// 使用Eigen库获取向量v1到向量v2的旋转四元数
    Eigen::Vector3d v1{vec1.x(), vec1.y(), vec1.z()};
    Eigen::Vector3d v2{vec2.x(), vec2.y(), vec2.z()};
    Eigen::Quaterniond qRotate = Eigen::Quaterniond::FromTwoVectors(v1, v2);

    /// 转换成OSG库里的四元数
    osg::Quat rotate(qRotate.x(), qRotate.y(), qRotate.z(), qRotate.w());

    /// 创建一个胶囊体对象记录圆柱体信息
    osg::ref_ptr<osg::Capsule> capsule = new osg::Capsule(point1, radius, len);

    /// 旋转胶囊体
    capsule->setRotation(rotate);

    /// 根据信息创建一个图形渲染对象
    osg::ref_ptr<osg::ShapeDrawable> capsuleDrawable =  new osg::ShapeDrawable(capsule, pHints);

    return capsuleDrawable;

}


void DrawPathWidget::close(const QString& fileName)
{

    if (fileName.isEmpty()) return;

    /// 访问器遍历找到对应名字的轨迹节点地址
    FindNodeWithName vistor(fileName.toStdString());
    pViewer->getSceneData()->asGroup()->accept(vistor);
    osg::ref_ptr<osg::Node> node = vistor.getNode();

    /// 删除节点
    pViewer->getSceneData()->asGroup()->removeChild(node);
    this->fileName = "";

}



void DrawPathWidget::open(const QString& fileDir)
{

#ifndef NDEBUG

        qDebug() << QStringLiteral("DrawPathWidget::open");

#endif

    if (fileDir.isEmpty()) return;
    this->fileName = "";

    /// 从文件路径fileDir提取文件名称fileName
    int cnt = 0;
    int idx = fileDir.size() - 1;
    while (idx >= 0 && fileDir[idx--] != '/') cnt++;
    this->fileName = fileDir.right(cnt).toStdString();

    if (!readFile(fileDir)) return;

}


/** 读取文件并解析文件
 * @fileName 文件名称
 * @return 文件打开c成功返回true
*/
bool DrawPathWidget::readFile(const QString& fileName)
{

#ifndef NDEBUG
        qDebug() << QStringLiteral("DrawPathWidget::readFile");
#endif

    this->pointsArray->clear();

    QFile file(fileName);
    QTextStream in(&file);

    if (!file.open(QFile::ReadOnly | QFile::Text)) {

#ifndef NDEBUG
        qDebug() << fileName << QStringLiteral("txt文件打开失败");
#endif

        return false;

    }

    /// 按行读取txt文件 按制表符分割字符串提取其中的数字加入数组中
    std::vector<std::vector<float>> points(0);

    while (!in.atEnd()) {

        QString line = in.readLine();
        if (line.isEmpty()) continue;

        /// 本txt文件是以制表符作为分割 不是空格
        std::vector<float> point = splitString(line, '\t');

        if (point.size() != 3) continue;
        points.push_back(point);

    }

    /// 更新数组缓存并绘制轨迹
    setPathPoint(points);

    file.close();
    return true;
}


/** 分割字符串
 * @line 一行字符串
 * @ch 分隔符
 */
std::vector<float> DrawPathWidget::splitString(const QString& line, const char& ch)
{
    if (line.isEmpty()) return {};
    std::vector<float> point;
    int len = line.size();

    for (int i = 0; i < len; ++i) {
        QString str;
        while (i < len && line[i] != ch) {
            str += line[i++];
        }
        if (!str.isEmpty()) {
            point.push_back(str.toFloat());
        }
    }
    return point;
}





/** 输出求交器与轨迹的所有相交点
 * @startPoint endPoint 求交平面的底线起点和终点
 * @upVector 求交平面从底线射向的方向
 * @return 相交点数组
 */
QVector<osg::Vec3d> DrawPathWidget::getIntersectorPoint(const osg::Vec3d& startPoint, const osg::Vec3d& endPoint, const osg::Vec3d& upVector)
{

    /// 储存相交点
    QVector<osg::Vec3d> intersectorPoints;

    /// 先创建个求交面
    osg::ref_ptr<osgUtil::PlaneIntersector> planeIntersector = createPlaneIntersector(startPoint, endPoint, upVector);

    /// 用来遍历相交节点访问器 通过回调机制将信息又返回存储在相交器中
    osgUtil::IntersectionVisitor intersectionVisitor(planeIntersector.get());
    pViewer->getCamera()->accept(intersectionVisitor);

    /// 判断是否有相交的节点
    if (planeIntersector->containsIntersections()) {

#ifndef NDEBUG

        qDebug() << QStringLiteral("DrawPathWidget::createPlaneIntersector(): 找到相交点");

#endif
        /// 从Intersections对象中获取交到的所有内容
        osgUtil::PlaneIntersector::Intersections& intersections = planeIntersector->getIntersections();


#if 1   /// 因为线半径0.1 因此误差小于等于0.2的都算同一个点
        /// 设计算法去除误差小于0.25的点

        /// 记录所有获得的交点
        QVector<osg::Vec3d> points;
        for (auto intersection : intersections) {

            for (auto p : intersection.polyline) {
                points.push_back(p);
            }

        }

        /// 按照点z轴的高度从小到大进行排序
        std::sort(points.begin(), points.end(), [](osg::Vec3d p1, osg::Vec3d p2){ return p1[2] < p2[2];} );

        /// 记录第一个点
        osg::Vec3d point = points[0];
        intersectorPoints.push_back(unMapping(point));
        drawPoint(point);

        for (auto p : points) {

            /// 发现大于0.25的点
            if (qAbs(p[2] - point[2]) > 0.25) {

                point = p;
                ///qDebug() << unMapping(point).x() << unMapping(point).y() << unMapping(point).z();
                intersectorPoints.push_back(unMapping(point));
                drawPoint(point);

            } else {    /// 把差值小于等于0.25的点去掉

                continue;

            }

        }

#else   /// 小部分临界状态一个轨迹可能对应2个相交点

        /// 遍历每一个相交对象 因为轨迹是一个封闭圆柱体 与求交面相交时实际上碰撞两次 因此检测到的物体数量为实际的2倍
        /// 例如只有一条轨迹 但是相交结果显示轨迹正面和背面的两个相交点
        /// 由于每条轨迹都能获得两个相交点 因此我们计算时取这两个交点的中点作为最终的实际相交点
        for (int i = 0; i < intersections.size()-1; i+=2) {

            /// intersection.polyline为面面相交的多段线的轨迹点数组 这里我们每次只取相交线上的第一个点
            osg::Vec3d& point1 = *intersections[i].polyline.begin();
            osg::Vec3d& point2 = *intersections[i+1].polyline.begin();
            osg::Vec3d mid = (point2 + point1) / 2;

            intersectorPoints.push_back(mid);

            /// 相交点绘制球体用于可视化
            drawPoint(mid);

        }

#endif

    }

    return intersectorPoints;

}



/** 创建平面求交器
 * @startPoint endPoint 求交平面的底线起点和终点
 * @upVector 求交平面从底线射向的方向
 */
osg::ref_ptr<osgUtil::PlaneIntersector> DrawPathWidget::createPlaneIntersector(const osg::Vec3d& startPoint,
                                                                               const osg::Vec3d& endPoint,
                                                                               const osg::Vec3d& upVector)
{

#ifndef NDEBUG

    qDebug() << "DrawPathWidget::createPlaneIntersector()";

#endif

    /// 创建求交平面和约束多面体
    osg::Plane plane;
    osg::Polytope boundingPolytope;

    /// 构造求交平面（点法式）
    osg::Vec3d planeNormal = (endPoint - startPoint) ^ upVector;
    planeNormal.normalize();
    plane.set(planeNormal, startPoint);

    /// 第一个约束面
    osg::Vec3d startPlaneNormal = upVector ^ planeNormal;   /// ^这是叉乘的意思
    startPlaneNormal.normalize();
    boundingPolytope.add(osg::Plane(startPlaneNormal, startPoint));

    /// 第二个约束面
    osg::Vec3d endPlaneNormal = planeNormal ^ upVector;
    endPlaneNormal.normalize();
    boundingPolytope.add(osg::Plane(endPlaneNormal, endPoint));

    /// 构造平面求交器
    osg::ref_ptr<osgUtil::PlaneIntersector> planeIntersector = new osgUtil::PlaneIntersector(plane, boundingPolytope);

    return planeIntersector;

}



/** 画一个几何球体
 * @point 球体的坐标
*/
void DrawPathWidget::drawPoint(const osg::Vec3d& point)
{

#ifndef NDEBUG

        qDebug() << QStringLiteral("DrawPathWidget::drawPoint");

#endif

    /// 绘制点的半径大小
    double radius = 0.3;

    osg::ref_ptr<osg::Geode> geode = new osg::Geode;

    /// 创建专门指明精细度的类osg::TessellationHints，并设置对应精细度
    osg::ref_ptr<osg::TessellationHints> pHints = new osg::TessellationHints;
    pHints->setDetailRatio(0.5);

    /// 绘制几何类型(几何体)
    geode->addDrawable(new osg::ShapeDrawable(new osg::Sphere(point, radius), pHints));

    /// 节点加入根节点中
    pViewer->getSceneData()->asGroup()->addChild(geode);

}




